
#ifndef __COMM_GEOMETRY_CPP__
#define __COMM_GEOMETRY_CPP__

#include "comm/comm_geometry.h"
#include "comm/comm_base.h"
#include <string.h>
#include <math.h>
#include <stdio.h>
#include <iostream>
#include <list>
#include <vector>

using namespace std;

VECTOR_C::VECTOR_C(void)
{
    isValid_ = false;
}

VECTOR_C::VECTOR_C(const COORDINATE_2D_STRU &stStartPoint, const COORDINATE_2D_STRU &stEndPoint)
{
    stPoint_.x = stEndPoint.x - stStartPoint.x;
    stPoint_.y = stEndPoint.y - stStartPoint.y;

    isExistPoint_  = true;
    isExistDirLen_ = false;
    isValid_       = true;
}

VECTOR_C::VECTOR_C(float len, float dirAngle)
{
    len_      = len;
    dirAngle_ = dirAngle;

    isExistPoint_  = false;
    isExistDirLen_ = true;
    isValid_       = true;
}

/**************************************************************************************
功能描述: 判断方向是否一致
修改记录:
**************************************************************************************/
BOOL VECTOR_C::IsDirConsistent(VECTOR_C &objOther)
{
    R_ASSERT(isValid_, false);

    return (DotMult(objOther) > 0);
}

float VECTOR_C::GetDirAngle(void)
{
    R_ASSERT(isValid_, 0.0);

    if (!isExistDirLen_) {
        ToDirLen();
    }

    return dirAngle_;
}

float VECTOR_C::GetLenSqu(void) const
{
    R_ASSERT(isValid_, -1);

    if (isExistPoint_) {
        return (stPoint_.x * stPoint_.x + stPoint_.y * stPoint_.y);
    } else {
        return len_ * len_;
    }
}

float VECTOR_C::DotMult(VECTOR_C &objOther)
{
    R_ASSERT(isValid_ && objOther.isValid_, 0.0);

    /* 优先坐标计算，再次长度和角度计算，最后转成坐标进行计算 */
    if (isExistPoint_ && objOther.isExistPoint_) {
        return (stPoint_.x * objOther.stPoint_.x + stPoint_.y * objOther.stPoint_.y);

    } else if (isExistDirLen_ && isExistDirLen_) {
        return (len_ * objOther.len_ *cos(dirAngle_ - objOther.dirAngle_));

    } else {
        if (isExistPoint_) {
            objOther.ToPoint();
        } else {
            ToPoint();
        }

        return (stPoint_.x * objOther.stPoint_.x + stPoint_.y * objOther.stPoint_.y);
    }
}

DIRECTED_LINE_C::DIRECTED_LINE_C(void)
{
    Clear();
}

DIRECTED_LINE_C::DIRECTED_LINE_C(const COORDINATE_2D_STRU &stStartPoint, const COORDINATE_2D_STRU &stEndPoint)
{
    Reset(stStartPoint, stEndPoint);
}

DIRECTED_LINE_C::DIRECTED_LINE_C(const COORDINATE_2D_STRU &stStartPoint, float dirAngle)
{
    Reset(stStartPoint, dirAngle);
}

void DIRECTED_LINE_C::Reset(const COORDINATE_2D_STRU &stStartPoint, const COORDINATE_2D_STRU &stEndPoint)
{
    stStartPoint_.Copy(stStartPoint);

    GetVector(stStartPoint_, stEndPoint, stVector_);
}

void DIRECTED_LINE_C::Reset(const COORDINATE_2D_STRU &stEndPoint)
{
    GetVector(stStartPoint_, stEndPoint, stVector_);
}

void DIRECTED_LINE_C::Reset(const COORDINATE_2D_STRU &stStartPoint, float dirAngle)
{
    stStartPoint_.Copy(stStartPoint);

    stVector_.x = cos(dirAngle);
    stVector_.y = sin(dirAngle);
}

void DIRECTED_LINE_C::Reset(const COORDINATE_2D_STRU &stStartPoint, float dirAngleSin, float dirAngleCos)
{
    stStartPoint_.Copy(stStartPoint);

    stVector_.x = dirAngleCos;
    stVector_.y = dirAngleSin;
}

void DIRECTED_LINE_C::Clear(void)
{
    stStartPoint_.SetZero();
    stVector_.SetZero();
}

BOOL DIRECTED_LINE_C::IsValid(void) const
{
    return (stVector_.GetVectorLenSqu() > 0);
}

/**************************************************************************************
功能描述: 判断给定点是否在该有向直线的左侧
修改记录:
**************************************************************************************/
bool DIRECTED_LINE_C::IsPointOnLeft(const COORDINATE_2D_STRU &stPoint) const
{
    /* 采用向量叉乘来判断 */
    return (GetPointVectorCrossMulti(stPoint) > 0.0);
}

/**************************************************************************************
功能描述: 判断给定点是否在该有向直线上
修改记录:
**************************************************************************************/
bool DIRECTED_LINE_C::IsPointOnLine(const COORDINATE_2D_STRU &stPoint) const
{
    /* 采用向量叉乘来判断 */
    return (GetPointVectorCrossMulti(stPoint) == 0.0);
}

/**************************************************************************************
功能描述: 判断给定点是否在该有向直线的右侧
修改记录:
**************************************************************************************/
bool DIRECTED_LINE_C::IsPointOnRight(const COORDINATE_2D_STRU &stPoint) const
{
    /* 采用向量叉乘来判断 */
    return (GetPointVectorCrossMulti(stPoint) < 0.0);
}

/**************************************************************************************
功能描述: 获取两条直线的交点
修改记录:
**************************************************************************************/
int DIRECTED_LINE_C::GetIntersectionWithAnotherLine(const DIRECTED_LINE_C &objLine2, COORDINATE_2D_STRU &stIntersection) const
{
    /* 判断是否平行，平行无交点 */
    float denominator = objLine2.stVector_.x * stVector_.y - objLine2.stVector_.y * stVector_.x;
    if (ABS(denominator) < 0.000001) {
        return -1;
    }

    float factor = ((stStartPoint_.x - objLine2.stStartPoint_.x) * stVector_.y - (stStartPoint_.y - objLine2.stStartPoint_.y) * stVector_.x) / denominator;
    stIntersection.x = objLine2.stStartPoint_.x + factor * objLine2.stVector_.x;
    stIntersection.y = objLine2.stStartPoint_.y + factor * objLine2.stVector_.y;
    return 0;
}

/**************************************************************************************
功能描述: 获取给定点到直线的垂直距离的平方
修改记录:
**************************************************************************************/
float DIRECTED_LINE_C::GetDistSquToPoint(const COORDINATE_2D_STRU &stPoint) const
{
    float lineVectorLenSqu = stVector_.GetVectorLenSqu();

    /* 如果表示直行反向的向量长度几乎为0，则无法确定直行方向，无法计算点到该直行的距离。 */
    if (IS_FLOAT_ALMOST_EQUAL_ZERO(lineVectorLenSqu)) {
        return -1;
    }

    /* 根据两个叉乘的结果数字上等于对应平行四边形面积，来计算点到直线的距离 */
    COORDINATE_2D_STRU stPointVector;
    GetVector(stStartPoint_, stPoint, stPointVector);

    float crossMulti = stVector_.x * stPointVector.y - stPointVector.x * stVector_.y;
    return (crossMulti * crossMulti) / lineVectorLenSqu;
}

float DIRECTED_LINE_C::GetDirAngle(void) const
{
    return atan2(stVector_.y, stVector_.x);
}

void DIRECTED_LINE_C::GetVector(const COORDINATE_2D_STRU &stPoint1, const COORDINATE_2D_STRU &stPoint2, COORDINATE_2D_STRU &stVector) const
{
    stVector.x = stPoint2.x - stPoint1.x;
    stVector.y = stPoint2.y - stPoint1.y;
}

/**************************************************************************************
功能描述: 获取给定点向量与原直线方向向量的叉乘结果
修改记录:
**************************************************************************************/
float DIRECTED_LINE_C::GetPointVectorCrossMulti(const COORDINATE_2D_STRU &stPoint) const
{
    /* 给定点向量为直行参考点到给定点的向量 */
    COORDINATE_2D_STRU stPointVectot;
    GetVector(stStartPoint_, stPoint, stPointVectot);

    return (stVector_.x * stPointVectot.y - stVector_.y * stPointVectot.x);
}

const COORDINATE_2D_STRU &RAY_C::GetStartPoint(void) const
{
    return stStartPoint_;
}

const COORDINATE_2D_STRU &DIRECTED_LINE_SEGMENT_C::GetEndPoint(void) const
{
    return stEndPoint_;
}

/**************************************************************************************
功能描述: 判断该线段是否和给定射线相交
修改记录:
**************************************************************************************/
bool DIRECTED_LINE_SEGMENT_C::IsIntersectedWithRay(const RAY_C &objRay) const
{
    const COORDINATE_2D_STRU &stRayStartPoint = objRay.GetStartPoint();
    if        (this->IsPointOnLeft(stRayStartPoint)) {
        return (!objRay.IsPointOnLeft(this->GetStartPoint())  && !objRay.IsPointOnRight(this->stEndPoint_));
    } else if (this->IsPointOnRight(stRayStartPoint)) {
        return (!objRay.IsPointOnRight(this->GetStartPoint()) && !objRay.IsPointOnLeft(this->stEndPoint_));
    } else {
        return true;
    }
}

int DIRECTED_LINE_SEGMENT_C::GetIntersectionWithRay(const RAY_C &objRay, COORDINATE_2D_STRU &stIntersection) const
{
    if (!IsIntersectedWithRay(objRay)) {
        return -1;
    }

    return GetIntersectionWithAnotherLine(objRay, stIntersection);
}

BOOL DIRECTED_LINE_SEGMENT_C::IsIntersectedWithOtherLineSeg(const DIRECTED_LINE_SEGMENT_C &objOther) const
{
    return (   (IsPointOnLeft(objOther.GetStartPoint()) != IsPointOnLeft(objOther.stEndPoint_))
            && (objOther.IsPointOnLeft(GetStartPoint()) != objOther.IsPointOnLeft(stEndPoint_)));
}

MULT_DIRECTED_LINE_C::MULT_DIRECTED_LINE_C(void)
{
    isFirstPointExist_ = false;
}

MULT_DIRECTED_LINE_C::~MULT_DIRECTED_LINE_C(void)
{
    Clear();
}

void MULT_DIRECTED_LINE_C::AddExtendPoint(const COORDINATE_2D_STRU &stNewPoint)
{
    if (isFirstPointExist_) {
        DIRECTED_LINE_C *pstNewLine = new DIRECTED_LINE_C(stLastPoint_, stNewPoint);
        if (pstNewLine == NULL) {
            ST_LOG_ERR("new fail, x=%f, y=%f.", stNewPoint.x, stNewPoint.y);
            return;
        }

        lstDirLine_.push_back(pstNewLine);
    }

    stLastPoint_.x = stNewPoint.x;
    stLastPoint_.y = stNewPoint.y;
    isFirstPointExist_ = true;
}

void MULT_DIRECTED_LINE_C::AddExtendPoint(float pointX, float pointY)
{
    COORDINATE_2D_STRU stNewPoint = {pointX, pointY};

    AddExtendPoint(stNewPoint);
}

void MULT_DIRECTED_LINE_C::Clear(void)
{
    for (list<DIRECTED_LINE_C *>::iterator iter = lstDirLine_.begin();
         iter != lstDirLine_.end();
         iter++) {
        delete *iter;
    }

    lstDirLine_.clear();
}

bool MULT_DIRECTED_LINE_C::IsPointOnLeft(const COORDINATE_2D_STRU &stPoint)
{
    if (lstDirLine_.size() == 0) {
        return false;
    }

    for (list<DIRECTED_LINE_C *>::iterator iter = lstDirLine_.begin();
         iter != lstDirLine_.end();
         iter++) {
        DIRECTED_LINE_C *pobjDirLine = *iter;
        if (!pobjDirLine->IsPointOnLeft(stPoint)) {
            return false;
        }
    }

    return true;
}

BOOL ANY_POLYGON_C::IsValid(void) const
{
    return (vectDirSide_.size() > 0);
}

BOOL ANY_POLYGON_C::IsPointIn(const COORDINATE_2D_STRU &stPoint) const
{
    R_ASSERT(vectDirSide_.size() > 0, false);

    /* 根据计算从指定点开始沿x周正方向的射线与多边形相交的边的个数来判断是否在内部，
       个数为奇数表示在内部，个数为偶数表示在外部。 */
    COORDINATE_2D_STRU stForward;
    stForward.x = stPoint.x + 1.0;
    stForward.y = stPoint.y;
    RAY_C objRay(stPoint, stForward);

    BOOL isIn = false;
    for (UINT32 loop = 0; loop < vectDirSide_.size(); ++loop) {
        const DIRECTED_LINE_SEGMENT_C &objSide = vectDirSide_[loop];
        if (objSide.IsIntersectedWithRay(objRay)) {
            isIn = !isIn;
        }
    }

    return isIn;
}

INT32 ANY_POLYGON_C::Reset(const COORDINATE_2D_STRU astPoint[], UINT32 pointNum)
{
    INT32 rslt = Make(astPoint, pointNum);
    if (rslt != 0) {
        Clear();
        ST_LOG_ERR("Make() Fail.");
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 判断新边是否合法
修改记录:
**************************************************************************************/
BOOL ANY_POLYGON_C::IsNewSideValid(const DIRECTED_LINE_SEGMENT_C &objNewSide, BOOL isLastSide) const
{
    for (UINT32 loop = 0; loop < vectDirSide_.size(); ++loop) {
        /* 所有边，除了和上一条边以及最后一条边和第一条边外，不能有交点，否则不合法 */
        if ((loop == (vectDirSide_.size() - 1)) || (isLastSide && (loop == 0))) {
            continue;
        }

        if (objNewSide.IsIntersectedWithOtherLineSeg(vectDirSide_[loop])) {
            return false;
        }
    }

    return true;
}

/**************************************************************************************
功能描述: 增加新的顶点
修改记录:
**************************************************************************************/
INT32 ANY_POLYGON_C::AddVertice(const COORDINATE_2D_STRU &stPoint, BOOL isLastPoint)
{
    if (!isExistFirstVertice_) {
        stLastVertice_.Copy(stPoint);
        isExistFirstVertice_ = true;
        return 0;
    }

    /* 新的顶点与上一个顶点构成一条新的边 */
    DIRECTED_LINE_SEGMENT_C objNewSide(stLastVertice_, stPoint);
    if (!IsNewSideValid(objNewSide, isLastPoint)) {
        ST_LOG_ERR("The New Vertice(x=%f, y=%f) Is Invalid", stPoint.x, stPoint.y);
        return -1;
    }

    vectDirSide_.push_back(objNewSide);
    stLastVertice_.Copy(stPoint);
    return 0;
}

void ANY_POLYGON_C::DspAllVertice(const COORDINATE_2D_STRU astPoint[], UINT32 pointNum) const
{
    ST_LOG_INFO("Vertice Num = %u.", pointNum);

    for (UINT32 loop = 0; loop < pointNum; ++loop) {
        ST_LOG_INFO("%3uth Verice x=%f, y=%f.", loop, astPoint[loop].x, astPoint[loop].y);
    }
}

INT32 ANY_POLYGON_C::Make(const COORDINATE_2D_STRU astPoint[], UINT32 pointNum)
{
    UINT32 verticeNum;
    if (astPoint[0].IsAlmostEuqal(astPoint[pointNum - 1])) {
        verticeNum = pointNum - 1;
    } else {
        verticeNum = pointNum;
    }

    if (verticeNum < 3) {
        ST_LOG_ERR("Vertice Num(%d) Is Not Enough.", verticeNum);
        DspAllVertice(astPoint, pointNum);
        return -1;
    }

    for (UINT32 loop = 0; loop < verticeNum; ++loop) {
        INT32 rslt = AddVertice(astPoint[loop], false);
        if (rslt != 0) {
            ST_LOG_ERR("AddVertice(%uth) Fail.", loop);
            DspAllVertice(astPoint, pointNum);
            return -1;
        }
    }

    INT32 rslt = AddVertice(astPoint[0], true);
    if (rslt != 0) {
        ST_LOG_ERR("AddVertice(last one) Fail.");
        DspAllVertice(astPoint, pointNum);
        return -1;
    }

    return 0;
}

void ANY_POLYGON_C::Clear(void)
{
    isExistFirstVertice_ = false;
    vectDirSide_.clear();
}

CONVEX_POLYGON_C::CONVEX_POLYGON_C(COORDINATE_2D_STRU astPoint[], int pointNum)
{
    isExistFirstVertice_ = false;
    stLastVertice_.SetZero();

    Make(astPoint, pointNum);
}

CONVEX_POLYGON_C::CONVEX_POLYGON_C(void)
{
    isExistFirstVertice_ = false;
    stLastVertice_.SetZero();
}

CONVEX_POLYGON_C::~CONVEX_POLYGON_C(void)
{
    Clear();
}

/**************************************************************************************
功能描述: 重新设置多边形
修改记录:
**************************************************************************************/
int CONVEX_POLYGON_C::Reset(COORDINATE_2D_STRU astPoint[], int pointNum)
{
    isExistFirstVertice_ = false;
    stLastVertice_.SetZero();

    Clear();

    return Make(astPoint, pointNum);
}

/**************************************************************************************
功能描述: 判断指定点是否在多边形内部
修改记录:
**************************************************************************************/
bool CONVEX_POLYGON_C::IsPointIn(const COORDINATE_2D_STRU &stPoint) const
{
    if (lstDirLineSeg_.empty()) {
        ST_LOG_ERR("No Polygon.");
        return false;
    }

    for (list<DIRECTED_LINE_SEGMENT_C *>::const_iterator iter = lstDirLineSeg_.begin();
         iter != lstDirLineSeg_.end();
         iter++) {
        DIRECTED_LINE_SEGMENT_C *pobjLineSeg = *iter;
        if (!pobjLineSeg->IsPointOnLeft(stPoint)) {
            return false;
        }
    }

    return true;
}

/**************************************************************************************
功能描述: 获取多边形内点沿某个方向到多边形边的长度
          调用方保证给定点一定为内点
修改记录:
**************************************************************************************/
float CONVEX_POLYGON_C::GetDistSquOfInternalPointAlongDirToEdge(const COORDINATE_2D_STRU &stInternalPoint, float dirAngle)
{
    RAY_C objRay(stInternalPoint, dirAngle);

    DIRECTED_LINE_SEGMENT_C *pObjIntersectedLineSeg = GetLineSegIntersectedByInternalRay(objRay);
    if (pObjIntersectedLineSeg == NULL) {
        ST_LOG_ERR("Can Not Find Inersected Seg.");
        return -1.0;
    }

    COORDINATE_2D_STRU stIntersection;
    if (0 != pObjIntersectedLineSeg->GetIntersectionWithAnotherLine(objRay, stIntersection)) {
        ST_LOG_ERR("Get Intersection Fail.");
        return -1.0;
    }

    return stInternalPoint.CalcDistSqu(stIntersection);
}

/**************************************************************************************
功能描述: 获取指定内点到多边形最近的距离
          由调用方保证给定点为多边形内点
          返回值为负表示异常，计算失败
修改记录:
**************************************************************************************/
float CONVEX_POLYGON_C::GetMinDistSquOfInternalPointToEdge(const COORDINATE_2D_STRU &stInternalPoint)
{
    float minDistSqu = -1.0;

    for (list<DIRECTED_LINE_SEGMENT_C *>::iterator iter = lstDirLineSeg_.begin();
         iter != lstDirLineSeg_.end();
         iter++) {
        DIRECTED_LINE_SEGMENT_C *pobjLineSeg = *iter;
        float distSqu = pobjLineSeg->GetDistSquToPoint(stInternalPoint);
        if ((minDistSqu < 0.0) || (distSqu < minDistSqu)) {
            minDistSqu = distSqu;
        }
    }

    return minDistSqu;
}

/**************************************************************************************
功能描述: 给多边形逆时针依次增加顶点
修改记录:
**************************************************************************************/
int CONVEX_POLYGON_C::AddVertice(COORDINATE_2D_STRU &stPoint)
{
    if (!isExistFirstVertice_) {
        stLastVertice_.Copy(stPoint);
        isExistFirstVertice_ = true;
        return 0;
    }

    /* 如果已存在边了，则需要校验下一个顶点是否合法，即在前一个边的左边，防止出现凹的，或不是顶点 */
    if (!lstDirLineSeg_.empty()) {
        DIRECTED_LINE_SEGMENT_C *pObjLastLineSeg = lstDirLineSeg_.back();
        if (pObjLastLineSeg == NULL) {
            ST_LOG_ERR("Get Last Seg Fail. Seg Num = %u.", UINT32(lstDirLineSeg_.size()));
            return -1;
        }

        if (!pObjLastLineSeg->IsPointOnLeft(stPoint)) {
            ST_LOG_ERR("New Vertice Not No Left.");
            return -1;
        }
    }

    DIRECTED_LINE_SEGMENT_C *pObjNewLineSeg = new DIRECTED_LINE_SEGMENT_C(stLastVertice_, stPoint);
    if (pObjNewLineSeg == NULL) {
        ST_LOG_ERR("Alloc New Line Seg Fail.");
        return -1;
    }

    lstDirLineSeg_.push_back(pObjNewLineSeg);
    stLastVertice_.Copy(stPoint);
    return 0;
}

/**************************************************************************************
功能描述: 检查该多边形是否合法
修改记录:
**************************************************************************************/
bool CONVEX_POLYGON_C::IsValid(void)
{
    /* 至少是3条边 */
    if (lstDirLineSeg_.size() < 3) {
        ST_LOG_ERR("Seg Num(%u) Is Less.", UINT32(lstDirLineSeg_.size()));
        return false;
    }

    /* 检查是否是封闭的 */
    DIRECTED_LINE_SEGMENT_C *pobjFirstLineSeg = lstDirLineSeg_.front();
    DIRECTED_LINE_SEGMENT_C *pobjLastLineSeg  = lstDirLineSeg_.back();
    const COORDINATE_2D_STRU &stFirstVertice = pobjFirstLineSeg->GetStartPoint();
    const COORDINATE_2D_STRU &stLastVertice  = pobjLastLineSeg->GetEndPoint();
    if (!stFirstVertice.IsAlmostEuqal(stLastVertice)) {
        ST_LOG_ERR("Not Closed.");
        return false;
    }

    /* 检查是否是凸的，即判断所有点都不在所有边的右边 */
    for (list<DIRECTED_LINE_SEGMENT_C *>::iterator iter1 = lstDirLineSeg_.begin();
         iter1 != lstDirLineSeg_.end();
         iter1++) {
        DIRECTED_LINE_SEGMENT_C *pobjLineSeg1 = *iter1;
        for (list<DIRECTED_LINE_SEGMENT_C *>::iterator iter2 = lstDirLineSeg_.begin();
             iter2 != lstDirLineSeg_.end();
             iter2++) {
            DIRECTED_LINE_SEGMENT_C *pobjLineSeg2 = *iter2;
            const COORDINATE_2D_STRU &stLineSegStartPoint = pobjLineSeg2->GetStartPoint();
            if (pobjLineSeg1->IsPointOnRight(stLineSegStartPoint)) {
                ST_LOG_ERR("Not Convex.");
                return false;
            }
        }
    }

    return true;
}

void CONVEX_POLYGON_C::Clear(void)
{
    for (list<DIRECTED_LINE_SEGMENT_C *>::iterator iter = lstDirLineSeg_.begin();
         iter != lstDirLineSeg_.end();
         iter++) {
        delete *iter;
    }

    lstDirLineSeg_.clear();
}

/**************************************************************************************
功能描述: 构造凸多边形
          各个顶点逆时针顺序构成多边形
修改记录:
**************************************************************************************/
int CONVEX_POLYGON_C::Make(COORDINATE_2D_STRU astPoint[], int pointNum)
{
    if (pointNum < 3) {
        ST_LOG_ERR("Vertice Num(%d) Is Not Enough.", pointNum);
        return -1;
    }

    for (int loop = 0; loop < pointNum; loop++) {
        if (0 != AddVertice(astPoint[loop])) {
            ST_LOG_ERR("Add Vertice(Idx=%d) Fail.", loop);
            Clear();
            return -1;
        }
    }

    /* 如果最后一个点和第一个点不相等，则再增加第一个点，使多边形闭合 */
    if (!astPoint[0].IsAlmostEuqal(astPoint[pointNum - 1])) {
        //ST_LOG_INFO("Not Closed, Add First Vertice.");
        if (0 != AddVertice(astPoint[0])) {
            ST_LOG_ERR("Add First Vertice Fail.");
            Clear();
            return -1;
        }
    }

    if (!IsValid()) {
        ST_LOG_ERR("Is Invalid.");
        Clear();
        return -1;
    }

    return 0;
}

/**************************************************************************************
功能描述: 获取多边形内点沿某个方向与多边形的边相交，获取该边的线段
          调用方保证给定点一定为内点
修改记录:
**************************************************************************************/
DIRECTED_LINE_SEGMENT_C *CONVEX_POLYGON_C::GetLineSegIntersectedByInternalRay(RAY_C &objInternalRay)
{
    for (list<DIRECTED_LINE_SEGMENT_C *>::iterator iter = lstDirLineSeg_.begin();
         iter != lstDirLineSeg_.end();
         iter++) {
        DIRECTED_LINE_SEGMENT_C *pobjLineSeg = *iter;
        if (pobjLineSeg->IsIntersectedWithRay(objInternalRay)) {
            return pobjLineSeg;
        }
    }

    return NULL;
}

/**************************************************************************************
功能描述: 根据三角形的两条边和夹角求第三条边长的平方
修改记录:
**************************************************************************************/
float TRIANGLE_C::Get3thSideLenSqu(float sideLen1, float sideLen2, ANGLE_C &objAngleOf2Sides)
{
    return (sideLen1 * sideLen1 + sideLen2 * sideLen2 - 2.0 * sideLen1 * sideLen2 * objAngleOf2Sides.Cos());
}

CIRCULAR_C::CIRCULAR_C(const COORDINATE_2D_STRU &stCenter, float radius)
{
    R_ASSERT(radius > 0.0, NOTHING);

    stCenter_ = stCenter;
    radius_   = radius;

    isValid_  = true;
}

CIRCULAR_C::CIRCULAR_C(const COORDINATE_2D_STRU &stPoint1, const COORDINATE_2D_STRU &stPoint2, const COORDINATE_2D_STRU &stPoint3)
{
    /* 三点不能太近 */
    COORDINATE_2D_STRU stVector12(stPoint2.x - stPoint1.x, stPoint2.y - stPoint1.y);
    COORDINATE_2D_STRU stVector23(stPoint3.x - stPoint2.x, stPoint3.y - stPoint2.y);
    COORDINATE_2D_STRU stVector13(stPoint3.x - stPoint1.x, stPoint3.y - stPoint1.y);
    if (   (stVector12.GetVectorLenSqu() < 1.0e-10)
        || (stVector23.GetVectorLenSqu() < 1.0e-10)
        || (stVector13.GetVectorLenSqu() < 1.0e-10)) {
        return;
    }

    /* 三点不能共线 */
    float det = stVector12.x * stVector23.y - stVector12.y * stVector23.x;
    if (ABS(det) < 1.0e-10) {
        return;
    }

    /* 计算圆心 */
    float e    = ((stPoint2.x * stPoint2.x - stPoint1.x * stPoint1.x) + (stPoint2.y * stPoint2.y - stPoint1.y * stPoint1.y)) * 0.5;
    float f    = ((stPoint2.x * stPoint2.x - stPoint3.x * stPoint3.x) + (stPoint2.y * stPoint2.y - stPoint3.y * stPoint3.y)) * 0.5;
    float detR = 1.0 / det;
    stCenter_.x =  (stVector23.y * e + stVector12.y * f) * detR;
    stCenter_.y = -(stVector23.x * e + stVector12.x * f) * detR;

    /* 计算半径 */
    COORDINATE_2D_STRU stVector01(stPoint1.x - stCenter_.x, stPoint1.y - stCenter_.y);
    radiusSqu_ = stVector01.GetVectorLenSqu();

    isValid_ = true;
}

BOOL CIRCULAR_C::IsValid(void) const
{
    return isValid_;
}

INT32 CIRCULAR_C::GetCenter(COORDINATE_2D_STRU &stCenter) const
{
    R_ASSERT(isValid_, -1);

    stCenter = stCenter_;
    return 0;
}

float CIRCULAR_C::GetRadiusSqu(void) const
{
    R_ASSERT(isValid_, -1);
    return radiusSqu_;
}

float CIRCULAR_C::GetRadius(void)
{
    R_ASSERT(isValid_, -1);

    if (!isExistRadius_) {
        radius_        = sqrt(radiusSqu_);
        isExistRadius_ = true;
    }

    return radius_;
}

INT32 CIRCULAR_C::GetSamplePoints(const COORDINATE_2D_STRU &stStartPoint, const COORDINATE_2D_STRU &stEndPoint, float deltaRadAbs, vector<COORDINATE_2D_STRU> &vectSamplePoints)
{
    R_ASSERT(isValid_, -1);

    VECTOR_C objVectorS(stCenter_, stStartPoint);
    VECTOR_C objVectorE(stCenter_, stEndPoint);

    float startAngle = objVectorS.GetDirAngle();
    float endAngle   = objVectorE.GetDirAngle();
    float diffAngle  = AdjustAngle(endAngle - startAngle);
    float symbol     = (diffAngle >= 0) ? 1.0 : -1.0;
    INT32 sampleNum  = INT32(ABS(diffAngle) / deltaRadAbs);
    float sinStart   = sin(startAngle);
    float cosStart   = cos(startAngle);
    float sinDelta   = sin(deltaRadAbs);
    float cosDelta   = cos(deltaRadAbs);
    float radius     = GetRadius();

    vectSamplePoints.reserve(sampleNum + 1);

    float sinSample = sinStart;
    float cosSample = cosStart;
    for (INT32 loop = 1; loop <= sampleNum; ++loop) {
        float sinSamplePre = sinSample;
        float cosSamplePre = cosSample;
        sinSample = sinSamplePre * cosDelta + cosSamplePre * sinDelta * symbol;
        cosSample = cosSamplePre * cosDelta - sinSamplePre * sinDelta * symbol;

        COORDINATE_2D_STRU stSamplePoint;
        stSamplePoint.x = stCenter_.x + radius * cosSample;
        stSamplePoint.y = stCenter_.y + radius * sinSample;

        vectSamplePoints.push_back(stSamplePoint);
    }

    vectSamplePoints.push_back(stEndPoint);
    return 0;
}


#endif


